#include "src/misc.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <thread>
#include <chrono>
#include <ctime>
using namespace std;
using namespace cv;
using namespace Eigen;
using namespace std::chrono;

RNG rng(5);
void withMatrix(TGPoint& P0, vector<TGPoint>& pts, int s, int e, double& ans);

void display() {
    vector<TGPoint> pts;
    Mat backGround;
    backGround = Mat(300+100, 400+100, CV_8UC3, White);
    int n = 8;

    //随机生成n个点集，其中前n-1个点构成首尾相连的折线段，最后一个点为定点
    generateRandomPointSet(n, rng, pts);
    pair<TGPoint, double>ans;
    ans.second = INT32_MAX;
    //计算离定点最近的折线段距离
    for(int i = 1; i < n-1; i ++) {
        auto tmp = pdist(pts.back(), {pts[i-1], pts[i]});
        //cout << tmp.second << endl;
        if(tmp.second < ans.second) {
            ans = tmp;
        }
    }

    //绘制折线段
    for(int i = 1; i < n-1; i ++)
        line(backGround, {pts[i-1].x, pts[i-1].y}, {pts[i].x, pts[i].y}, Blue, 2);
    
    //绘制定点以及该点关于最近折线的垂足
    circle(backGround, {pts.back().x, pts.back().y}, 3, Green, -1);
    line(backGround, {pts.back().x, pts.back().y}, {ans.first.x, ans.first.y}, Red, 2);
    imshow("Main", backGround);
    waitKey(0);
    destroyAllWindows();
}


void P2L(TGPoint& P, vector<TGPoint>& polyLine, int s, int e, double& ans) {
    ans = INT_MAX;
    for(int i = s; i < e; i ++) {
        ans = min(ans, pdist(P, {polyLine[i], polyLine[i+1]}).second);
    }
}

double withMultiThread(TGPoint& P, vector<TGPoint>& polyLine) {
    int den = 4;
    int n = polyLine.size();
    int m = n / den;
    vector<double>ans(den, INT_MAX);
    thread pool[den];
    for(int i = 0; i < den; i ++) {
        int s = i*m;
        int e = (i+1)*m;
        if(i+1 == den) e = n-1;
        pool[i] = thread(P2L, ref(P), ref(polyLine), s, e, ref(ans[i]));
    }
    for(auto& p : pool) {
        p.join();
    }
    return *min_element(ans.begin(), ans.end());
}

double withMultiThread1(TGPoint& P, vector<TGPoint>& polyLine) {
    int den = 4;
    int n = polyLine.size();
    int m = n / den;
    vector<double>ans(den, INT_MAX);
    thread pool[den];
    for(int i = 0; i < den; i ++) {
        int s = i*m;
        int e = (i+1)*m;
        if(i+1 == den) e = n-1;
        pool[i] = thread(withMatrix, ref(P), ref(polyLine), s, e, ref(ans[i]));
    }
    for(auto& p : pool) {
        p.join();
    }
    return *min_element(ans.begin(), ans.end());
}

void withMatrix(TGPoint& P0, vector<TGPoint>& pts, int s, int e, double& ans) {
    int n = e-s+1;
    Vector2d P;
    // MatrixXd P(1, 2);
    MatrixXd A(2, n);
    P << P0.x, P0.y;
    for(int i = s; i <= e; i ++) {
        A(0, i-s) = pts[i].x;
        A(1, i-s) = pts[i].y;
    }

    //计算定点P到折线段各个端点的最小距离
    // auto _P = P.transpose() * MatrixXd::Ones(1, A.cols()); //2*A.cols()
    // auto B = _P - A;
    auto B = -(A.colwise()-P); //2*A.cols()
    // cout << "P: \n" << _P << endl;
    // cout << "A: \n" << A << endl;
    // cout << "P-A: \n" << _P.cwiseProduct(A) << endl;
    auto P2P = B.cwiseProduct(B).colwise().sum().cwiseSqrt(); //点到折线端点的距离
    // cout << "点到直线的距离:\n" << P2P << endl;
    // cout << "点到折线端点的距离\n";
    // cout << P2P << endl;

    // MinInfo m1;
    // m1.v = M1.minCoeff(&m1.row, &m1.col);


    //计算定点到各个折线段垂足的最小距离
    auto A1 = A.block(0, 0, 2, A.cols()-1); //2*A.cols()-1
    auto A2 = A.block(0, 1, 2, A.cols()-1); //2*A.cols()-1
    // auto tP = _P.block(0, 0, 2, A.cols()-1);
    // auto V1 = tP - A1, V3 = tP-A2;
    auto V1 = -(A1.colwise()-P), V3 = -(A2.colwise()-P);
    auto V2 = A2 - A1;

    auto D1 = V1.cwiseProduct(V2).colwise().sum();
    auto D2 = V3.cwiseProduct(-V2).colwise().sum();

    auto Area = V1.row(0).cwiseProduct(V2.row(1)) - V1.row(1).cwiseProduct(V2.row(0));
    auto _A = A1 - A2;
    auto LineLength = _A.cwiseProduct(_A).colwise().sum().cwiseSqrt();
    auto P2L = Area.cwiseAbs().cwiseProduct(LineLength.cwiseInverse()); //点到直线的距离

    // cout << "Area\n";
    // cout << Area.cwiseAbs() << endl;
    // cout << "LineLength\n";
    // cout << LineLength << endl;
    // cout << "点到折线段距离\n" << P2L << endl;

    double retM = P2P.minCoeff();
    // auto _q = (D1.array() < 0).select(retM, P2L);
    // auto qq = (D2.array() < 0).select(retM, _q);    
    // retM = qq.minCoeff();
    // cout << "....Begin For-Loop...\n";

    auto ncol = P2L.cols();
    for(int i = 0; i < ncol; i ++) {
        // if(i%100 == 0) printf("%d, ", i);
        if(D1(0, i) < 0 || D2(0, i) < 0) continue;
        retM = min(retM, P2L(0, i));
    }
    // cout << "矩阵法点到折线段的最近距离为: " << retM << endl;

    ans = retM;
}

double withMatrix1(TGPoint& P0, vector<TGPoint>& pts) {
    int n = pts.size();
    MatrixXd P(1, 2);
    MatrixXd A(2, n);
    P << P0.x, P0.y;
    for(int i = 0; i < n; i ++) {
        A(0, i) = pts[i].x;
        A(1, i) = pts[i].y;
    }

    //计算定点P到折线段各个端点的最小距离
    auto _P = P.transpose() * MatrixXd::Ones(1, A.cols()); //2*A.cols()
    auto B = _P - A;
    auto P2P = B.cwiseProduct(B).colwise().sum().cwiseSqrt(); //点到折线端点的距离

    //计算定点到各个折线段垂足的最小距离
    auto A1 = A.block(0, 0, 2, A.cols()-1); //2*A.cols()-1
    auto A2 = A.block(0, 1, 2, A.cols()-1); //2*A.cols()-1
    auto tP = _P.block(0, 0, 2, A.cols()-1);
    auto V1 = tP - A1, V3 = tP-A2;
    auto V2 = A2 - A1;

    auto D1 = V1.cwiseProduct(V2).colwise().sum();
    auto D2 = V3.cwiseProduct(-V2).colwise().sum();

    auto Area = V1.row(0).cwiseProduct(V2.row(1)) - V1.row(1).cwiseProduct(V2.row(0));
    auto _A = A1 - A2;
    auto LineLength = _A.cwiseProduct(_A).colwise().sum().cwiseSqrt();
    auto P2L = Area.cwiseAbs().cwiseProduct(LineLength.cwiseInverse()); //点到直线的距离
    double retM = P2P.minCoeff();
    auto _q = (D1.array() < 0).select(retM, P2L);
    auto qq = (D2.array() < 0).select(retM, _q);    
    retM = qq.minCoeff();
    return retM;
}


int main() {

    vector<TGPoint> _pts;
    int n = 20000000;
    //随机生成n个点集，其中前n-1个点构成首尾相连的折线段，最后一个点为定点
    generateRandomPointSet(n, rng, _pts);
    vector<TGPoint>pts;
    pts.reserve(n);
    pts.emplace_back(_pts[0]);
    //去除重复点
    for(int i = 1; i < n; i ++) {
        if(equal(_pts[i], pts.back())) continue;
        pts.emplace_back(_pts[i]);
    }

    auto P = pts.back(); pts.pop_back();
    n = pts.size();

    double ans1=INT_MAX, ans2=INT_MAX, ans3=INT_MAX, ans4=INT_MAX, ans5=INT_MAX;
    auto s = chrono::system_clock::now();
    P2L(P, pts, 0, n-1, ans1);
    auto e = chrono::system_clock::now();
    cout << "BruteForce: " << duration_cast<milliseconds>(e-s).count() << "ms" << endl;

    s = system_clock::now();
    ans2 = withMultiThread(P, pts);
    e = system_clock::now();
    cout << "MultiThread: " << duration_cast<milliseconds>(e-s).count() << "ms" << endl;

    s = system_clock::now();
    ans3 = withMatrix1(P, pts);
    e = system_clock::now();
    cout << "MatrixBenchmark: " << duration_cast<milliseconds>(e-s).count() << "ms" << endl;

    s = system_clock::now();
    withMatrix(P, pts, 0, n-1, ans4);
    e = system_clock::now();
    cout << "Matrix: " << duration_cast<milliseconds>(e-s).count() << "ms" << endl;

    s = system_clock::now();
    ans5 = withMultiThread1(P, pts);
    e = system_clock::now();
    cout << "MultiMatrix: " << duration_cast<milliseconds>(e-s).count() << "ms" << endl;

    cout << dcmp(ans1, ans2) << endl;
    cout << dcmp(ans1, ans3) << endl;
    cout << dcmp(ans1, ans4) << endl;
    cout << dcmp(ans1, ans5) << endl;

    return 0;
}